//! Lidar control client implementation
//!
//! This module provides the LidarClient for controlling Unitree lidar
//! start/stop functionality through DDS communication.

use r2r::std_msgs::msg::String as StringMsg;
use r2r::{Node, Publisher, QosProfile};
use std::sync::{Arc, Mutex};

/// Lidar control commands
#[derive(Debug, Clone, PartialEq)]
pub enum LidarCommand {
    /// Turn on the lidar
    On,
    /// Turn off the lidar
    Off,
}

impl LidarCommand {
    /// Convert command to string message
    pub fn to_string(&self) -> String {
        match self {
            LidarCommand::On => "ON".to_string(),
            LidarCommand::Off => "OFF".to_string(),
        }
    }
}

/// Lidar control client
pub struct LidarClient {
    publisher: Arc<Mutex<Publisher<StringMsg>>>,
    node: Arc<Mutex<Node>>,
}

impl LidarClient {
    /// Create a new lidar client
    ///
    /// # Arguments
    /// * `node_name` - Name of the ROS2 node
    /// * `namespace` - Namespace for the node (can be empty)
    ///
    /// # Returns
    /// A tuple containing the LidarClient and the Node
    pub fn new(node_name: &str, namespace: &str) -> Result<Self, Box<dyn std::error::Error>> {
        let ctx = r2r::Context::create()?;
        let node = r2r::Node::create(ctx, node_name, namespace)?;

        let node_arc = Arc::new(Mutex::new(node));

        // Create publisher for lidar control topic
        let publisher = node_arc
            .lock()
            .unwrap()
            .create_publisher::<StringMsg>("rt/utlidar/switch", QosProfile::default())?;

        Ok(Self {
            publisher: Arc::new(Mutex::new(publisher)),
            node: node_arc.clone(),
        })
    }

    pub fn from_node(node: &mut Arc<Mutex<r2r::Node>>) -> Result<Self, r2r::Error> {
        let publisher = node
            .lock()
            .unwrap()
            .create_publisher::<StringMsg>("rt/utlidar/switch", QosProfile::default())?;
        Ok(Self {
            publisher: Arc::new(Mutex::new(publisher)),
            node: node.clone(),
        })
    }

    /// Send lidar control command
    ///
    /// # Arguments
    /// * `command` - The lidar command to send (On/Off)
    ///
    /// # Returns
    /// Result indicating success or failure
    pub fn send_command(&self, command: LidarCommand) -> Result<(), Box<dyn std::error::Error>> {
        let mut msg = StringMsg::default();
        msg.data = command.to_string();

        self.publisher.lock().unwrap().publish(&msg)?;
        println!("Lidar command sent: {}", command.to_string());

        Ok(())
    }

    /// Turn on the lidar
    pub fn turn_on(&self) -> Result<(), Box<dyn std::error::Error>> {
        self.send_command(LidarCommand::On)
    }

    /// Turn off the lidar
    pub fn turn_off(&self) -> Result<(), Box<dyn std::error::Error>> {
        self.send_command(LidarCommand::Off)
    }
}
